cmake_minimum_required(VERSION 3.8)
project(common)

# Default to C++17
if(NOT CMAKE_CXX_STANDARD)
  set(CMAKE_CXX_STANDARD 17)
endif()

if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
  add_compile_options(-Wall -Wextra -Wpedantic)
endif()

set(CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake)

# Find dependencies
find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(visualization_msgs REQUIRED)
find_package(sensor_msgs REQUIRED)
find_package(geometry_msgs REQUIRED)
find_package(tf2 REQUIRED)
find_package(tf2_ros REQUIRED)

find_package(Protobuf REQUIRED)
find_package(Eigen3 REQUIRED)
find_package(OpenCV REQUIRED)
find_package(PkgConfig REQUIRED)
pkg_check_modules(GLOG REQUIRED libglog)

# Third party includes
set(THIRD_PARTY_INCLUDE_DIRS ${PROJECT_SOURCE_DIR}/thirdparty)
set(THIRD_PARTY_INCLUDE_DIRS  ${THIRD_PARTY_INCLUDE_DIRS} ${PROTOBUF_INCLUDE_DIR})
set(OOQP_INCLUDE_DIRS ${PROJECT_SOURCE_DIR}/thirdparty/ooqp/include/ooqp)

# Find OOQP library (optional)
find_package(OOQP)
if(OOQP_FOUND)
    message(STATUS "OOQP found. Building with OOQP support.")
    set(BUILD_WITH_OOQP TRUE)
else()
    message(WARNING "OOQP not found. Building without OOQP support. Some features may be disabled.")
    set(BUILD_WITH_OOQP FALSE)
endif()

# Include directories
set(COMMON_INCLUDE_DIRS
  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
  $<INSTALL_INTERFACE:include>
  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/thirdparty>
  $<INSTALL_INTERFACE:include/thirdparty>
)

set(THIRD_PARTY_LIBRARIES
    ${GLOG_LIBRARIES}
    ${PROTOBUF_LIBRARY}
)

include_directories(
    ${COMMON_INCLUDE_DIRS}
)

# Create OOQP interface library (if OOQP is available)
if(BUILD_WITH_OOQP)
    add_library(hkust_pl_ooqp_itf SHARED
        src/solver/ooqp_interface.cc
        src/solver/qp_solver.cc
    )
    # Enable position independent code for shared libraries
    set_target_properties(hkust_pl_ooqp_itf PROPERTIES
        POSITION_INDEPENDENT_CODE ON
    )
    target_include_directories(hkust_pl_ooqp_itf PUBLIC
        $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
        $<INSTALL_INTERFACE:include>
        $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/thirdparty>
        $<INSTALL_INTERFACE:include/thirdparty>
        $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/thirdparty/ooqp/include/ooqp>
        $<INSTALL_INTERFACE:include/thirdparty/ooqp/include/ooqp>
        ${EIGEN3_INCLUDE_DIR}
    )
    target_link_libraries(hkust_pl_ooqp_itf
        ${OOQP_LIBRARIES}
    )
    ament_target_dependencies(hkust_pl_ooqp_itf
        rclcpp
    )
    set(OOQP_TARGET hkust_pl_ooqp_itf)
else()
    # Create dummy OOQP interface when OOQP is not available
    add_library(hkust_pl_ooqp_itf INTERFACE)
    set(OOQP_TARGET)
endif()
set(COMMON_SRC
    src/basics/colormap.cc
    src/basics/semantics.cc
    src/basics/shapes.cc
    src/basics/tool_func.cc
    src/math/calculations.cc
    src/rss/rss_checker.cc
    src/idm/intelligent_driver_model.cc
    src/mobil/mobil_model.cc
    src/mobil/mobil_behavior_prediction.cc
    src/spline/lookup_table.cc
    # src/spline/spline_generator.cc  # May depend on OOQP
    src/lane/lane.cc
    src/lane/lane_generator.cc
    src/primitive/frenet_primitive.cc
    src/state/state_transformer.cc
    src/state/free_state.cc
    src/circle_arc/circle_arc.cc
    src/circle_arc/circle_arc_branch.cc
)

# Add OOQP-dependent sources if available
if(BUILD_WITH_OOQP)
    set(COMMON_SRC ${COMMON_SRC}
        src/spline/spline_generator.cc
        # Add other OOQP-dependent sources here
    )
endif()

# Create main common library
add_library(hkust_pl_common SHARED
    ${COMMON_SRC}
)
# Enable position independent code for shared libraries
set_target_properties(hkust_pl_common PROPERTIES
    POSITION_INDEPENDENT_CODE ON
)
target_include_directories(hkust_pl_common PUBLIC
    $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
    $<INSTALL_INTERFACE:include>
    $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/thirdparty>
    $<INSTALL_INTERFACE:include/thirdparty>
    ${EIGEN3_INCLUDE_DIR}
)
target_link_libraries(hkust_pl_common
    ${THIRD_PARTY_LIBRARIES}
    ${OpenCV_LIBS}
)
if(BUILD_WITH_OOQP)
    target_link_libraries(hkust_pl_common hkust_pl_ooqp_itf)
endif()
ament_target_dependencies(hkust_pl_common
    rclcpp
    visualization_msgs
    sensor_msgs
    geometry_msgs
    tf2
    tf2_ros
)

# Install libraries
install(TARGETS hkust_pl_common hkust_pl_ooqp_itf
  EXPORT export_${PROJECT_NAME}
  ARCHIVE DESTINATION lib
  LIBRARY DESTINATION lib
  RUNTIME DESTINATION bin
)

# Install headers
install(DIRECTORY include/
  DESTINATION include
)

# Install third party headers
install(DIRECTORY thirdparty/
  DESTINATION include/thirdparty
)

# Export dependencies
ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET)
ament_export_dependencies(
  rclcpp
  visualization_msgs
  sensor_msgs
  geometry_msgs
  tf2
  tf2_ros
  Eigen3
)

# Export include directories
ament_export_include_directories(include)

# Linting
if(BUILD_TESTING)
  find_package(ament_lint_auto REQUIRED)
  ament_lint_auto_find_test_dependencies()
  
  # Add simple build test (temporarily disabled for migration)
  # add_executable(test_build test_build.cpp)
  # target_link_libraries(test_build hkust_pl_common)
  # ament_target_dependencies(test_build rclcpp)
  
  # install(TARGETS test_build
  #   DESTINATION lib/${PROJECT_NAME}
  # )
endif()

ament_package()
